#include "physicstests.h"

#include "inputs/manager.h"
#include "application.h"
#include "graphics/graphicsmanager.h"

#include "shapes.h"
#include "debugdraw.h"

#include "ServiceCamera.h"

namespace infinity {

PhysicsTests::PhysicsTests()
:	GameMode("PhysicsTests")
,	m_cursorShapeType(dw::maths::Shape::Type_Sphere)
{
	m_trackMouse.addSampler(dw::inputs::MouseCursorSampler(dw::inputs::MouseCursorSampler::X, dw::inputs::MouseCursorSampler::Absolute));
	m_trackMouse.addSampler(dw::inputs::MouseCursorSampler(dw::inputs::MouseCursorSampler::Y, dw::inputs::MouseCursorSampler::Absolute));
	m_trackMouse.addSampler(dw::inputs::MouseCursorSampler(dw::inputs::MouseCursorSampler::Z, dw::inputs::MouseCursorSampler::Relative));

	m_customState.value(dw::graphics::Renderer::RS_TEXTURE_2D) = false;
	m_customState.value(dw::graphics::Renderer::RS_CULLING) = true;
	m_customState.value(dw::graphics::Renderer::RS_SHADE_MODE) = dw::graphics::Renderer::ShadeMode::GOURAUD;
	m_customState.value(dw::graphics::Renderer::RS_FILL_MODE) = dw::graphics::Renderer::FillMode::SOLID;
	m_customState.value(dw::graphics::Renderer::RS_LIGHTING) = false;
	m_customState.value(dw::graphics::Renderer::RS_DEPTH_TEST) = true;
	m_customState.value(dw::graphics::Renderer::RS_DEPTH_FUNC) = dw::graphics::Renderer::LESS;

	// Spheres

	dw::maths::Sphere* smallSphere = new dw::maths::Sphere();
	dw::maths::Sphere* avgSphere = new dw::maths::Sphere();
	dw::maths::Sphere* bigSphere = new dw::maths::Sphere();

	smallSphere->position = Vector3f(-5.0f, 0.0f, 0.0f);
	smallSphere->radius = 0.5f;

	avgSphere->position = Vector3f(0.0f, 0.0f, 0.0f);
	avgSphere->radius = 1.0f;

	bigSphere->position = Vector3f(5.0f, 0.0f, 0.0f);
	bigSphere->radius = 1.5f;

	m_shapes.push_back(smallSphere);
	m_shapes.push_back(avgSphere);
	m_shapes.push_back(bigSphere);

	// Boxes
	dw::maths::AABBox* smallBox = new dw::maths::AABBox();
	dw::maths::AABBox* avgBox = new dw::maths::AABBox();
	dw::maths::AABBox* bigBox = new dw::maths::AABBox();

	smallBox->position = Vector3f(-5.0f, 0.0f, -5.0f);
	smallBox->halfExtend = Vector3f(0.5f, 0.5f, 0.5f);

	avgBox->position = Vector3f(0.0f, 0.0f, -5.0f);
	avgBox->halfExtend = Vector3f(1.0f, 1.0f, 1.0f);

	bigBox->position = Vector3f(5.0f, 0.0f, -5.0f);
	bigBox->halfExtend = Vector3f(1.5f, 1.5f, 1.5f);

	m_shapes.push_back(smallBox);
	m_shapes.push_back(avgBox);
	m_shapes.push_back(bigBox);

	// Rays
	dw::maths::Ray* smallRay = new dw::maths::Ray();
	dw::maths::Ray* avgRay = new dw::maths::Ray();
	dw::maths::Ray* bigRay = new dw::maths::Ray();

	smallRay->position = Vector3f(-5.0f, 0.0f, 5.0f);
	smallRay->direction = Vector3f(2.3f, 0.0f, 0.0f);

	avgRay->position = Vector3f(0.0f, 0.0f, 5.0f);
	avgRay->direction = Vector3f(1.0f, 1.0f, 1.0f);

	bigRay->position = Vector3f(5.0f, 0.0f, 5.0f);
	bigRay->direction = Vector3f(1.5f, 1.5f, 1.5f);

	m_shapes.push_back(smallRay);
	m_shapes.push_back(avgRay);
	m_shapes.push_back(bigRay);
}

PhysicsTests::~PhysicsTests()
{
}

void PhysicsTests::install()
{
 	dw::inputs::InputManager& inputManager(dw::inputs::InputManager::getInstance());
 	
	inputManager.registerCommand(&m_trackMouse);
}

void PhysicsTests::update(const dw::float32 deltaT)
{
	ServiceCamera& svcCamera = Application::getInstance().getService< ServiceCamera >();
	dw::graphics::Node* cameraNode = svcCamera.getActiveCameraController()->getMatrix();

	if (m_trackMouse.isActive())
	{
		dw::float32 cursorX = m_trackMouse.getValue(0);
		dw::float32 cursorY = m_trackMouse.getValue(1);
		dw::float32 wheel = m_trackMouse.getValue(2);

		//! @todo retrieve ray direction from cursorX, cursorY

		const Vector3f position(cameraNode->getTransform().pos);
		const Vector3f direction(cameraNode->getTransform().at);

		const Vector3f planeNormal(0.0f, 1.0f, 0.0f);

		const dw::float32 planeDotOrigin = Vector3f::dot(planeNormal, position);
		const dw::float32 planeDotDirection = Vector3f::dot(planeNormal, direction);

		if (planeDotDirection < 0.0f)
		{
			dw::float32 distance = -planeDotOrigin / planeDotDirection;

			m_cameraPointer = position + distance * direction;
		}

		if (wheel != 0.0f)
		{
			m_cursorShapeType = (dw::maths::Shape::Type)(m_cursorShapeType + (dw::int32)dw::sign(wheel));

			if (m_cursorShapeType > dw::maths::Shape::Type_AABBox)
			{
				m_cursorShapeType = dw::maths::Shape::Type_Ray;
			}
			else if (m_cursorShapeType < dw::maths::Shape::Type_Ray)
			{
				m_cursorShapeType = dw::maths::Shape::Type_AABBox;
			}
		}
	}
}

static bool getIntersectionAndColor(const dw::maths::Shape& shapeA, const dw::maths::Shape& shapeB, dw::maths::Intersection& intersection, dw::graphics::Colorf& color)
{	
	if (dw::maths::Shape::intersects(shapeA, shapeB, intersection))
	{
		switch (intersection.type)
		{
		case  dw::maths::Intersection::Type_Intersects :
			color = dw::graphics::Colorf::Green;
			break ;
		case  dw::maths::Intersection::Type_IncludesB :
			color = dw::graphics::Colorf::Red;
			break ;
		case  dw::maths::Intersection::Type_IncludesA :
			color = dw::graphics::Colorf::Blue;
			break ;
		}

		return true;
	}

	color = dw::graphics::Colorf::Black;

	return false;
}

void PhysicsTests::render()
{
	static GLUquadric* gluSphereQuadric = NULL;
	ServiceCamera& svcCamera = Application::getInstance().getService< ServiceCamera >();
	dw::graphics::Camera* camera = svcCamera.getActiveCameraController()->getCamera();
	dw::graphics::Node* cameraNode = svcCamera.getActiveCameraController()->getMatrix();

	const dw::system::Window& window = Application::getInstance().getWindow();
	dw::graphics::Renderer& renderer(dw::graphics::GraphicsManager::getInstance().getRenderer());

	// Custom Render States
	renderer.pushState();
	renderer.setState(m_customState);

	// Projection & View matrices
	const Matrix4f projMtx = camera->getProjectionMatrix();
	const Matrix4f viewMtx = cameraNode->getViewMatrix();
	
	renderer.setMatrixMode(dw::graphics::Renderer::PROJECTION);
	renderer.loadIdentity();
	renderer.multMatrix(projMtx);

	renderer.setMatrixMode(dw::graphics::Renderer::MODELVIEW);
	renderer.loadIdentity();
	renderer.multMatrix(viewMtx);

	if (gluSphereQuadric == NULL)
	{
		gluSphereQuadric = gluNewQuadric();
		gluQuadricDrawStyle(gluSphereQuadric, GLU_LINE);
	}

	// draw a grid
	glColor3f( 0.2f, 0.2f, 0.2f );
	glBegin(GL_LINES);
	{
		for (int x = -10; x < 11; ++x)
		{
			glVertex3f((float)x, 0.0f, -11.0f);
			glVertex3f((float)x, 0.0f,  11.0f);
		}
		for (int y = -10; y < 11; ++y)
		{
			glVertex3f(-11.0f, 0.0f, (float)y);
			glVertex3f( 11.0f, 0.0f, (float)y);
		}
	}
	glEnd();

	glBegin(GL_LINES);
	{
		glColor3f( 1.0f, 0.0f, 0.0f );
		glVertex3f( 0.0f, 0.0f, 0.0f );
		glVertex3f( 1.0f, 0.0f, 0.0f );
	
		glColor3f( 0.0f, 1.0f, 0.0f );
		glVertex3f( 0.0f, 0.0f, 0.0f );
		glVertex3f( 0.0f, 1.0f, 0.0f );

		glColor3f( 0.0f, 0.0f, 1.0f );
		glVertex3f( 0.0f, 0.0f, 0.0f );
		glVertex3f( 0.0f, 0.0f, 1.0f );
	}
	glEnd();

	dw::maths::Shape* cursorShape;
	dw::maths::Ray cursorRay;
	dw::maths::Sphere cursorSphere;
	dw::maths::AABBox cursorAABBox;

	switch (m_cursorShapeType)
	{
	case dw::maths::Shape::Type_Ray :
		cursorRay.direction = Vector3f(2.0f, 0.0f, 0.0f);
		cursorRay.position = m_cameraPointer;
		cursorShape = &cursorRay;
		break ;
	case dw::maths::Shape::Type_Sphere :
		cursorSphere.radius = 1.0f;
		cursorSphere.position = m_cameraPointer;
		cursorShape = &cursorSphere;
		break ;
	case dw::maths::Shape::Type_AABBox :
		cursorAABBox.halfExtend = Vector3f(1.0f, 1.0f, 1.0f);
		cursorAABBox.position = m_cameraPointer;
		cursorShape = &cursorAABBox;
		break ;
	}

	// Intersects and render All shapes
	dw::maths::Intersection intersection;
	dw::maths::Intersection intersectionBackUp;

	dw::graphics::Colorf color;

	for (ShapesCollection::const_iterator shapeIt = m_shapes.begin(); shapeIt < m_shapes.end(); ++shapeIt)
	{
		const dw::maths::Shape* shape = *shapeIt;

		if (getIntersectionAndColor(*cursorShape, *shape, intersection, color))
		{
			intersectionBackUp = intersection;
		}

		glPushMatrix();
		if (shape->type == dw::maths::Shape::Type_Ray)
		{
			const dw::maths::Ray* ray = static_cast< const dw::maths::Ray* >(shape);

			glColor4fv(color.data);
			glTranslatef(ray->position.x, ray->position.y, ray->position.z);
			glBegin(GL_LINES);
			{
				glVertex3f(0.0f, 0.0f, 0.0f);
				glVertex3fv(ray->direction.v);
			}
			glEnd();
		}
		else if (shape->type == dw::maths::Shape::Type_Sphere)
		{
			const dw::maths::Sphere* sphere = static_cast< const dw::maths::Sphere* >(shape);

			glColor4fv(color.data);
			glTranslatef(sphere->position.x, sphere->position.y, sphere->position.z);
			gluSphere(gluSphereQuadric, sphere->radius, 16, 8);
		}
		else if (shape->type == dw::maths::Shape::Type_AABBox)
		{
			const dw::maths::AABBox* box = static_cast< const dw::maths::AABBox* >(shape);

			AABBox< dw::float32, 3 > drawBox;

			drawBox.min = box->position - box->halfExtend;
			drawBox.max = box->position + box->halfExtend;
			DebugDraw::render(color, drawBox);
		}
		glPopMatrix();
	}

	// Render cursor shape
	glColor3f(1.0f, 1.0f, 1.0f);

	glPushMatrix();
	if (cursorShape->type == dw::maths::Shape::Type_Ray)
	{
		const dw::maths::Ray* ray = static_cast< const dw::maths::Ray* >(cursorShape);

		glColor4fv(color.data);
		glTranslatef(ray->position.x, ray->position.y, ray->position.z);
		glBegin(GL_LINES);
		{
			glVertex3f(0.0f, 0.0f, 0.0f);
			glVertex3fv(ray->direction.v);
		}
		glEnd();
	}
	else if (cursorShape->type == dw::maths::Shape::Type_Sphere)
	{
		const dw::maths::Sphere* sphere = static_cast< const dw::maths::Sphere* >(cursorShape);

		glColor4fv(color.data);
		glTranslatef(sphere->position.x, sphere->position.y, sphere->position.z);
		gluSphere(gluSphereQuadric, sphere->radius, 16, 8);
	}
	else if (cursorShape->type == dw::maths::Shape::Type_AABBox)
	{
		const dw::maths::AABBox* box = static_cast< const dw::maths::AABBox* >(cursorShape);

		AABBox< dw::float32, 3 > drawBox;

		drawBox.min = box->position - box->halfExtend;
		drawBox.max = box->position + box->halfExtend;
		DebugDraw::render(color, drawBox);
	}
	glPopMatrix();

	if (intersectionBackUp.type != dw::maths::Intersection::Type_None)
	{
		Vector3f scaledNormal = intersectionBackUp.depth * intersectionBackUp.normalA;
		Vector3f fixedPosition = Vector3f::zero;

		glColor3f(1.0f, 1.0f, 0.0f);
		glBegin(GL_LINES);
		{
			Vector3f normalEnd = intersectionBackUp.position + scaledNormal;
			glVertex3f(intersectionBackUp.position.x, intersectionBackUp.position.y, intersectionBackUp.position.z);
			glVertex3f(normalEnd.x, normalEnd.y, normalEnd.z);
		}
		glEnd();
		
		dw::graphics::Colorf color(0.5f, 0.5f, 1.0f);
		// Shape once extracted
		glColor3fv(color.data);

		glPushMatrix();
		if (cursorShape->type == dw::maths::Shape::Type_Ray)
		{
			const dw::maths::Ray* ray = static_cast< const dw::maths::Ray* >(cursorShape);

			fixedPosition = ray->position - scaledNormal;

			glTranslatef(fixedPosition.x, fixedPosition.y, fixedPosition.z);
			glBegin(GL_LINES);
			{
				glVertex3f(0.0f, 0.0f, 0.0f);
				glVertex3fv(ray->direction.v);
			}
			glEnd();
		}
		else if (cursorShape->type == dw::maths::Shape::Type_Sphere)
		{
			const dw::maths::Sphere* sphere = static_cast< const dw::maths::Sphere* >(cursorShape);

			fixedPosition = sphere->position - scaledNormal;

			glTranslatef(fixedPosition.x, fixedPosition.y, fixedPosition.z);
			gluSphere(gluSphereQuadric, sphere->radius, 16, 8);
		}
		else if (cursorShape->type == dw::maths::Shape::Type_AABBox)
		{
			const dw::maths::AABBox* box = static_cast< const dw::maths::AABBox* >(cursorShape);

			fixedPosition = box->position - scaledNormal;

			AABBox< dw::float32, 3 > drawBox;

			drawBox.min = fixedPosition - box->halfExtend;
			drawBox.max = fixedPosition + box->halfExtend;
			DebugDraw::render(color, drawBox);
		}
		glPopMatrix();
	}

	renderer.popMatrix();
	renderer.popState();
}

void PhysicsTests::term()
{
	dw::inputs::InputManager& inputManager(dw::inputs::InputManager::getInstance());

	inputManager.unregisterCommand(&m_trackMouse);
}

} // namespace infinity
